/*
 * MotorController.h
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 */

#ifndef MOTORCONTROLLER_H_
#define MOTORCONTROLLER_H_

#define MAINFILE

#include <vector>
#include <ctime>
#include <iostream>
#include <fstream>
#include <cmath>

#define LEG_CUTOFF				12

#define TORQUE_ENABLE			24
#define MOVING_SPEED			32
#define GOAL_ACCELERATION		73

#define DEFAULT_MOVING_SPEED	50
#define DEFAULT_ACCELERATION	15
#define LOW_ACCELERATION		15
#define GOAL_POSITION			30
#define PRESENT_POSITION		36
#define RETURN_DELAY_TIME		5
#define STATUS_RETURN_LEVEL		16
#define MAX_TORQUE				14
#define TORQUE_LIMIT			34

#define STAND					0
#define WALK					1
#define TURNLEFT				2
#define TURNRIGHT				3
#define SIDESTEPLEFT			4
#define SIDESTEPRIGHT			5
#define KICK					6
#define ANIMATION				7
#define TEMP_ANIMATION			8


#define BYTE					1
#define WORD					2

#define BATTERY_TIMEOUT			20
#define VOLTAGE_THRESHHOLD		105

#define MOTION_FILE		"/home/mul8/Documents/motion_file.txt"

#define WALK_FILE		"/home/mul8/Documents/Walk_Forward.txt"
#define STAND_FILE		"/home/mul8/Documents/stand_file.txt"
#define TURN_LEFT_FILE	"/home/mul8/Documents/Turn_Left.txt"
#define TURN_RIGHT_FILE	"/home/mul8/Documents/Turn_Right.txt"
#define SIDE_STEP_LEFT_FILE		"/home/mul8/Documents/SideStepLeft.txt"
#define SIDE_STEP_RIGHT_FILE	"/home/mul8/Documents/SideStepRight.txt"
#define KICK_FILE		"/home/mul8/Documents/Kick.txt"
#define ANIMATION_FILE		"/home/mul8/Documents/animation.txt"
#define TEMP_ANIMATION_FILE		"/home/mul8/Documents/temp/animation.txt"

struct Motion {
	std::vector<double> time;						//Array of times for the motions
	std::vector<std::vector<int> > motorPositions;	//Array of motor positions for the motions
	std::vector<std::vector<int> > motorVelocities;	//Array of motor velocities for the motions

	std::vector<int>  currPos;		//Array for holding potential new positions
	std::vector<int>  recoverPos;		//Array for recovering a single step's positions
	std::vector<int>  recoverVel;		//Array for recovering a single step's velocity
	int recoverTime;		//recover a single step's time

	int currentIndex;	//step index. 0-n							//Integer marking the currently indexed step
	int length;			//number of steps in a motion							//Integer representing the size of the arrays
};

//#define std::vector<std::string> MOTION_FILES {STAND_FILE, WALK_FILE, TURN_LEFT_FILE, TURN_RIGHT_FILE};

class MotorController {
private:
	float timeSince(float);
	void initializeMotion(void);
	void initialize();
	Motion getInitializedMotion(const char*);
	void changePID();
	void setReturnDelayTime();
public:
	MotorController();
	virtual ~MotorController();
	void setMotion(int);
	int getMotion(void);
	bool step(bool);
	void sendSyncWrite(int[], int, int, int, std::vector<int>);
	void executeNext(Motion);
	bool incrementStep();
	Motion getMotionFile();
	void executePrevious(Motion);
	void disableMotor(int);
	void enableMotor(int);
	void disableAllMotors();
	void enableAllMotors();
	void incrementIndex();
	void lockHead();
	void disableLeftLeg();
	void disableRightLeg();
	void enableRightLeg();
	void enableLeftLeg();
	int readMotorPosition(int);
	int getMotorPositionReadWord(int);
	std::vector<int> getMotorPositionSyncRead(int);
	void setStatusReturnLevel(int);
	void getCurrentPose();
	int errorCheck(int[], int);
	void setTime(double); //takes new time input and passes it to editStep
	void editStep(Motion&, int, double, int[]); /*edit motion struct to replace a step and calc new vel with time input*/
	void printMotion(int); //this will print the current motion in "motion reading" format so the user can paste them into a txt file.
	void addMotionStep(void); //Add a step to the current motion
	void addMotion(void); // Add a new motion
	void decrementIndex(void); // Decrement step index


};

#endif /* MOTORCONTROLLER_H_ */
